#pragma once
#include <stdafx.h>
#include <QObject>
#include "PointCloudBase.h"


#include <mutex>  
#include "KinectDataCollection.h"
#include "MarchingCubesTSDF.h"

class DataAnalysis :public QObject, public PointCloudBase
{
	Q_OBJECT
	#define POINT_CLOUD_SCALE 2
	#define CLUSTER_DISTANCE 0.04f*POINT_CLOUD_SCALE
public:
	enum emState
	{
		STATE_OK=0,
		STATE_FaceNeed = 1,
		STATE_LeftViewsNeeded = 2,
		STATE_RightViewsNeeded = 4,
		STATE_MovingTooFast = 8,
		STATE_WAIT_START=16
	};
public:

	struct suKinectData
	{
		KinectDataCollection::suDepthData m_Depth2D;
	//	PPointCloudDef m_pPntClound;
	//	vector<bool> m_isValid;
		inline bool IsDataValid(int inx) const
		{
			bool ret = false;
			if (inx >=0&&inx <m_Depth2D.m_vData.size())
			{
				ret = m_Depth2D.m_vData[inx] > 0 ? true : false;
			}
			return ret;
		}
		inline int GetDataSize() const
		{
			return m_Depth2D.m_vData.size();
		}
		cv::Mat m_colorData;
		PPointCloudDef m_pFacePnt;
		Eigen::Matrix4Xf m_matFace;
	};

	struct suResData
	{
		PPointCloudDef m_pHeadData;
		cv::Mat m_ImgData;
		KinectDataCollection::suDepthData m_Depth2D;
		cv::Point2f m_pntEllipseMiddle;
		cv::Size m_pntEllipseLength;
		Vector4 m_veFaceOrientation;
	};

	struct suHeadData
	{
		suHeadData()
		{

		}
		suHeadData(CameraSpacePoint pntBase, const Vector4& ori, const KinectDataCollection::suDepthData& vDepthData, PPointCloudDef pHeadData, PPointCloudDef pFaceData)
		{
			m_pntFaceBase = pntBase;
			m_veFaceOrientation = ori;
			m_pHeadData = pHeadData;
			m_pFaceData = pFaceData;
			m_Depth2D = vDepthData;
		}
		Eigen::Matrix4Xf ToMat() const
		{
			Eigen::Translation3f translate(m_pntFaceBase.X, m_pntFaceBase.Y, m_pntFaceBase.Z);
			Eigen::Quaternion<float> veFaceOrientation(m_veFaceOrientation.w, m_veFaceOrientation.x, m_veFaceOrientation.y, m_veFaceOrientation.z);
			Eigen::Matrix4Xf mat = (translate * veFaceOrientation).matrix();
			return mat;
		}
		Eigen::Quaternion<float> ToQuaternion()
		{
			return Eigen::Quaternion<float>(m_veFaceOrientation.w, m_veFaceOrientation.x, m_veFaceOrientation.y, m_veFaceOrientation.z);
		}
		CameraSpacePoint		m_pntFaceBase;
		Vector4 m_veFaceOrientation;
		KinectDataCollection::suDepthData m_Depth2D;
		PPointCloudDef m_pHeadData;
		PPointCloudDef m_pFaceData;
	};
	DataAnalysis();

	~DataAnalysis();

	bool GetCurrentData(suResData&data, bool isRemove = true)
	{
		bool ret = false;
		m_dataLock.lock();
		if (!m_vResData.empty())
		{
			data = *(m_vResData.begin());
			ret = true;
			if (isRemove)
			{
				m_vResData.erase(m_vResData.begin());
			}
		}
		m_dataLock.unlock();
		return ret;
	}

	//QString GetCurrentFaceInfo()
	//{
	//	m_lockFaceInfo.lock();
	//	QString ret =  m_faceInfo;
	//	m_lockFaceInfo.unlock();
	//	return ret;
	//}

	void Start();
	void PrepareReStarDataCollect();

	void StartCollect();

	shared_ptr<pcl::PolygonMesh> GetFinalMesh()
	{
		/*	shared_ptr<pcl::PolygonMesh> pmesh = shared_ptr<pcl::PolygonMesh>(new pcl::PolygonMesh());
			LoadMeshAsBin("SaveData\\mesh.bin", *pmesh);
			return pmesh;*/
		return m_pMesh;
	
	}

	void Stop();
protected slots:
	void ProgressChange(float fNewProgressValue, QString strInfo)
{
	emit sign_ProgressChange(fNewProgressValue, strInfo);
}
	//void FaceInfoChange(QString str)
	//{
	//	m_lockFaceInfo.lock();
	//		m_faceInfo = str;
	//	m_lockFaceInfo.unlock();
	//}
signals:
	void sign_ResDataArrived();

	void sign_CalMeshFinal();

	void sign_ProgressChange(float fNewProgressValue, QString strInfo);

	void sign_StateChange(int newState, float fEndTime);
protected:
	void SetState(int iNewState, float fEndTime = -1)
	{
		bool isChange = true;
		if (m_State== STATE_WAIT_START &&( (iNewState&STATE_MovingTooFast)== STATE_MovingTooFast))
		{
			isChange = false;
		}
		if(isChange)
		{
			m_State = iNewState;
			emit sign_StateChange(m_State, fEndTime);

		}
	}
	void RunCalRes();
	bool IsFaceTransformEnough(const Eigen::Quaternion<float>& oriBefore, const Eigen::Quaternion<float>& oriNow, float dDetal=0.005);
	void ReveiceDataMain();
	void ReveiceData();
	std::vector<pair<int, int>>GetRegion(int x, int y, int iWidth, int iHeight);
	
	PPointCloudDef ExtractHeadData(const suKinectData& data);
	PPointCloudDef ExtractHeadRegion(const suKinectData& data);
	PCLPoint ToPCLPoint(const CameraSpacePoint& camPnt);

	cv::Mat HandleImage(const suKinectData& data);
	KinectDataCollection::suDepthData FilterData(const KinectDataCollection::suDepthData& dataIn);

	void InsertHeadData(const suHeadData& headData);
	void ClearHeadData();
	bool GetTopData(suHeadData&headData, bool IsNeedRemove = false);
	bool GetLastData(suHeadData&headData, bool IsNeedRemove = false);
	bool IsHeadEmpty();
	int GetHeadSize();
	void StarAnalysisData();

	bool AnalysisFaceOri(const Vector4& vNow, const Vector4& vStart);

	inline  float GetAngle(const Vector4& v1, const Vector4& v2) const
	{
		float fValue = v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
		float vL1 = v1.x*v1.x + v1.y*v1.y + v1.z*v1.z; vL1 = sqrt(vL1);
		float vL2 = v2.x*v2.x + v2.y*v2.y + v2.z*v2.z; vL2 = sqrt(vL2);
		fValue /= (vL1*vL2);
		fValue = std::min(1.0f, fValue);
		fValue = std::max(-1.0f, fValue);
		return acos(fValue) * 180 / 3.14159;

	}

	inline Vector4 UnitData(const Vector4& v1) const
	{
		float vL1 = v1.x*v1.x + v1.y*v1.y + v1.z*v1.z; vL1 = sqrt(vL1);
		Vector4 ret = v1;
		ret.x /= vL1;
		ret.y /= vL1;
		ret.z /= vL1;
		return ret;
	}
protected:
		shared_ptr<pcl::PolygonMesh> m_pMesh;
private:
	Vector4 m_posFaceBeginDir;
	clock_t m_startTime, m_secondTime;
	float m_lastAngle;

	atomic<int> m_State;

	QString m_faceInfo;
	atomic<int> m_IsStartCal;
	atomic<int> m_threadReveiceData, m_threadAnalysisData;
	cv::Size m_faceScope;
	cv::Point2f m_faceMiddle;
	list<suKinectData> m_vlsData;
	std::mutex m_dataLock, m_headLock, m_lockFaceInfo;
	list<suResData> m_vResData;
	list<suHeadData> m_vHeadData;
	MarchingCubesTSDF<PCLPoint> m_marchingCubes;
};

